
#include "fc_sensors.h"
// #include "lsm6dsr.h"
#include "usercmd.h"
#include "ZINO\filter\filter.h"
#include "shanwai.h"
#include "zino.h"
#include "ZINO\hardware\spl06\spl06_01.h"
#include <math.h>
#include <sensor.h>
#include "fc_error.h"
#include "imu.h"
#include "icm42688p_read_polling.h"
#include "statecal.h"
#include "filter.h"
#include "motorset.h"
#include "program.h"
#include "statecontrol.h"
#include "board.h"
#define DBG_TAG "sensors"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

extern float F1_wave[12];

fc_sensor_data_t fc_sensor_data;
zdrone_state_t zdrone_state;

bool baro_online = false;
bool acc_gyro_online = false;
bool laser_online = false;
bool opticalflow_online = false;
bool acc_hardfault = false;
bool gyro_hardfault = false;
static uint8_t acc_hardfault_cnt = 0;
static uint8_t gyro_hardfault_cnt = 0;
rt_device_t temp_dev = RT_NULL;
rt_device_t baro_dev = RT_NULL;
rt_device_t acce_dev = RT_NULL;
rt_device_t gyro_dev = RT_NULL;
rt_device_t tof_dev = RT_NULL;
rt_device_t optical_dev = RT_NULL;

struct rt_sensor_data tof_data;
struct rt_sensor_data optical_data;
struct rt_sensor_data temp_data;
struct rt_sensor_data baro_data;
struct rt_sensor_data acce_data;
struct rt_sensor_data gyro_data;

extern void flipinghandle_fun(void);
void sensordata_update(void *parameter)
{

#define TEMP_DEV "temp_spl"
#define BARO_DEV "baro_spl"
#define ACCE_DEV "acce_icm"
#define GYRO_DEV "gyro_icm"
#define TOF_DEV "tof_NDS0"
#define OPTICAL_DEV "of_pmw39"
#define RUNINNG_HZ(X) if (_1ms_cnt % (1000 / (X)) == 0)

    static uint16_t _1ms_cnt = 0;
    rt_thread_mdelay(5); // NOTE wait for serial empty

    if (ZeroSave()->gyro_calibrated && ZeroSave()->acc_calibrated && ZeroSave()->flag == ZERO_SAVED_FLAG)
    {
        LOG_D("Loading acc gyro bias:");
        LOG_D("ACC: %d %d %d", ZeroSave()->accoffset.x, ZeroSave()->accoffset.y, ZeroSave()->accoffset.z);
        LOG_D("GYRO: %d %d %d", ZeroSave()->gyrooffset.x, ZeroSave()->gyrooffset.y, ZeroSave()->gyrooffset.z);
        fc_sensor_data.acc.acc_offset.isdatacalibrated = true;
        fc_sensor_data.gyro.gyro_offset.isdatacalibrated = true;
    }
    else
    {
        ZeroSave()->acc_cutoff = ACC_LPF_CUTOFF;
        ZeroSave()->gyro_cutoff = GYRO_LPF_CUTOFF;
        LOG_D("No acc gyro bias");
        LOG_D("acc gyro start calibration");
    }
    uint8_t motion_cnt = 0;
    uint8_t motion_freq=0;
    const rt_base_t MOTION_PIN = 44;//pin num PC.12 -> 44
    rt_pin_mode(MOTION_PIN, PIN_MODE_INPUT_PULLUP);
    statecontrol_init();
    fc_motor_on();
    while (1)
    {
        uint32_t now_time = rt_tick_get();
        RUNINNG_HZ(100)
        {
            if (!baro_online)
            {
                FC_ERROR_SET(FC_BARO_HARDFAULT);
                baro_dev = rt_device_find(BARO_DEV);
                temp_dev = rt_device_find(TEMP_DEV);
                if (temp_dev != RT_NULL && baro_dev != RT_NULL)
                {
                    rt_device_open(baro_dev, RT_DEVICE_FLAG_RDWR);
                    rt_device_open(temp_dev, RT_DEVICE_FLAG_RDWR);
                    baro_online = true;
                    FC_ERROR_RESET(FC_BARO_HARDFAULT);
                    LOG_D("baro ok\n");
                }
            }
            else
            {
                rt_device_read(baro_dev, 0, &baro_data, 1);
                rt_device_read(temp_dev, 0, &temp_data, 1);
                sensor_baro_handle(&baro_data, &temp_data);
                // rt_kprintf("baro_data:%d\n",baro_data.data.baro);
            }
            static uint32_t last_time1 = 0;
            float dt = (float)(now_time - last_time1) * 0.001f;
            last_time1 = now_time;
            positionEstimate(&fc_sensor_data, &fc_ctrl_imu, &zdrone_state, dt);

            position_control(&fc_control, &setPoint, &fc_sensor_data, &zdrone_state, dt);

            /*NOTE SETPION
            FORWARD: +Pitch, BACKWARD: -Pitch
            LEFT: -Roll, RIGHT: +Roll
             */

            // rt_printf_float("sYaw: ",setPoint.Angle.yaw," \n");

            // rt_printf_float("Vx: ",zdrone_state.vel.x," ");
            // rt_printf_float("Vy: ",zdrone_state.vel.y," ");
            // rt_printf_float("VxO: ",pidData[PID_POSITION_VX].out," ");
            // rt_printf_float("VyO: ",pidData[PID_POSITION_VY].out," ");
            // rt_printf_float("sPitch: ",setPoint.Angle.pitch," ");
            // rt_printf_float("sRoll: ",setPoint.Angle.roll," \n");
            // F1_wave[0] = zdrone_state.vel.x;
            // F1_wave[1] = zdrone_state.vel.y;
            // F1_wave[2] = pidData[PID_POSITION_VX].out;
            // F1_wave[3] = pidData[PID_POSITION_VY].out;
            // F1_wave[4] = setPoint.Angle.pitch;
            // F1_wave[5] = setPoint.Angle.roll;
        }

        RUNINNG_HZ(1000)
        {
            static uint32_t last_time2 = 0;
            float dt = (float)(now_time - last_time2) * 0.001f;
            last_time2 = now_time;
            if (!acc_gyro_online)
            {
                FC_ERROR_SET(FC_ACC_GYRO_HARDFAULT);
                acce_dev = rt_device_find(ACCE_DEV);
                gyro_dev = rt_device_find(GYRO_DEV);
                if (acce_dev != RT_NULL && gyro_dev != RT_NULL)
                {
                    rt_device_open(acce_dev, RT_DEVICE_FLAG_RDWR);
                    rt_device_open(gyro_dev, RT_DEVICE_FLAG_RDWR);
                    acc_gyro_online = true;
                    FC_ERROR_RESET(FC_ACC_GYRO_HARDFAULT);
                    LOG_D("acc_gyro ok\n");
                }
            }
            else
            {
                acce_data.type = RT_SENSOR_CLASS_NONE;
                gyro_data.type = RT_SENSOR_CLASS_NONE;
                int ret_acc = rt_device_read(acce_dev, 0, &acce_data, 1);
                int ret_gyro = rt_device_read(gyro_dev, 0, &gyro_data, 1);

                sensor_acc_process(&acce_data);
                sensor_gyro_process(&gyro_data);

                if ((acce_data.type != RT_SENSOR_CLASS_ACCE) || (ret_acc != 1))
                {
                    if (acc_hardfault_cnt < 100)
                    {
                        acc_hardfault_cnt++;
                    }
                    else
                    {
                        acc_hardfault = true;
                    }
                }
                else
                {
                    acc_hardfault_cnt = 0;
                    acc_hardfault = false;
                }
                if ((gyro_data.type != RT_SENSOR_CLASS_GYRO) || (ret_gyro != 1))
                {
                    if (gyro_hardfault_cnt < 100)
                    {
                        gyro_hardfault_cnt++;
                    }
                    else
                    {
                        gyro_hardfault = true;
                    }
                }
                else
                {
                    gyro_hardfault_cnt = 0;
                    gyro_hardfault = false;
                }

                if (acc_hardfault || gyro_hardfault)
                {
                    FC_ERROR_SET(FC_ACC_GYRO_HARDFAULT);
                    if (FC_ERROR_CHECK(FC_ARMED))
                    {
                        FC_ERROR_RESET(FC_ARMED);
                    }
                }
                else
                {
                    FC_ERROR_RESET(FC_ACC_GYRO_HARDFAULT);
                }

                MahonyAHRS(&fc_ctrl_imu, &fc_sensor_data, dt);

                static uint16_t overContrl = 0;
                if (fc_control.MixerRange > 900)
                {
                    // LOG_D("MixerRange:%d",fc_control.MixerRange);
                    if (overContrl < 100)
                    {
                        overContrl++;
                    }
                    if (overContrl >= 100)
                    {
                        overContrl = 0;
                    }
                }
                else
                {
                    overContrl = 0;
                }
            }
            if (get_fc_main_state() != true)
            {
                /*翻转功能*/
                flipinghandle_fun();
                /*PID控制*/
                statecontrol(&fc_control, &setPoint, &fc_sensor_data, &zdrone_state, dt);
                /**电机控制 */
                fc_motor_set_output(&fc_control);
            }
        }

        RUNINNG_HZ(30)
        {
            if (!laser_online)
            {
                FC_ERROR_SET(FC_LASER_HARDFAULT);
                tof_dev = rt_device_find(TOF_DEV);
                if (tof_dev != RT_NULL)
                {
                    rt_device_open(tof_dev, RT_DEVICE_FLAG_RDWR);
                    laser_online = true;
                    FC_ERROR_RESET(FC_LASER_HARDFAULT);
                    // LOG_D("TOF ok\n");
                }
                rt_memset(&tof_data, 0, sizeof(tof_data));
            }
            else
            {
                if (rt_device_read(tof_dev, 0, &tof_data, 1) == 1)
                {
                    sensor_tof_handle(&tof_data);
                }
                else
                {
                    rt_memset(&tof_data, 0, sizeof(tof_data));
                }
            }
        }
        RUNINNG_HZ(1000)
        {
            if (!opticalflow_online)
            {
                FC_ERROR_SET(FC_OPTICALFLOW_HARDFAULT);
                optical_dev = rt_device_find(OPTICAL_DEV);
                if (optical_dev != RT_NULL)
                {
                    rt_device_open(optical_dev, RT_DEVICE_FLAG_RDWR);
                    opticalflow_online = true;
                    FC_ERROR_RESET(FC_OPTICALFLOW_HARDFAULT);
                    // LOG_D("optical ok\n");
                }
            }
            else
            {
                if(rt_pin_read(MOTION_PIN) == PIN_LOW)
                {
                    if (rt_device_read(optical_dev, 0, &optical_data, 1) == 1)
                    {
                        sensor_opticlFlow_handle(&optical_data);
                        motion_cnt ++;
                    }
                    else
                    {
                        rt_memset(&optical_data, 0, sizeof(optical_data));
                    }
                }
            }
        }

        _1ms_cnt++;

        if (_1ms_cnt >= 1000)
        {
            _1ms_cnt = 0;
            motion_freq = motion_cnt;
            motion_cnt = 0;
            // rt_kprintf("motion_freq:%d\n",motion_freq);
        }

        rt_thread_mdelay(1);
    }
}
/**
 * @description: 读取原始数据，把原始数据读到sensordata中
 * @return {*}
 */
int sensor_task_entry(void)
{
    rt_thread_t sensor_data_thread = rt_thread_create("sensors", sensordata_update, RT_NULL, 1024, 1, 1);
    if (sensor_data_thread != RT_NULL)
    {
        rt_thread_startup(sensor_data_thread);
    }
    else
    {
        LOG_E("sensor_data_thread create failed.");
    }
    return 0;
}
ZINO_APP_EXPORT(sensor_task_entry);
// MSH_CMD_EXPORT(sensordata_update, sensor task entry);
